diff --git a/arch/arm/include/asm/arch-mx7ulp/scg.h b/arch/arm/include/asm/arch-mx7ulp/scg.h
index f1fae01..531d8f3 100644
--- a/arch/arm/include/asm/arch-mx7ulp/scg.h
+++ b/arch/arm/include/asm/arch-mx7ulp/scg.h
@@ -337,5 +337,6 @@
 void scg_a7_sys_clk_sel(enum scg_sys_src clk);
 void scg_a7_info(void);
 void scg_a7_soscdiv_init(void);
+void scg_a7_init_core_clk(void);
 
 #endif
diff --git a/arch/arm/mach-imx/mx7ulp/clock.c b/arch/arm/mach-imx/mx7ulp/clock.c
index 7012157..7bf8317 100644
--- a/arch/arm/mach-imx/mx7ulp/clock.c
+++ b/arch/arm/mach-imx/mx7ulp/clock.c
@@ -300,6 +300,8 @@
 
 	scg_a7_soscdiv_init();
 
+	scg_a7_init_core_clk();
+
 	/* APLL PFD1 = 270Mhz, PFD2=345.6Mhz, PFD3=800Mhz */
 	scg_enable_pll_pfd(SCG_APLL_PFD1_CLK, 35);
 	scg_enable_pll_pfd(SCG_APLL_PFD2_CLK, 28);
diff --git a/arch/arm/mach-imx/mx7ulp/scg.c b/arch/arm/mach-imx/mx7ulp/scg.c
index a28a2bc..0d31352 100644
--- a/arch/arm/mach-imx/mx7ulp/scg.c
+++ b/arch/arm/mach-imx/mx7ulp/scg.c
@@ -1091,3 +1091,44 @@
 	debug("SCG RCCR Value: 0x%x\n", readl(&scg1_regs->rccr));
 	debug("SCG Clock Status: 0x%x\n", readl(&scg1_regs->csr));
 }
+
+void scg_a7_init_core_clk(void)
+{
+	u32 val = 0;
+
+	/*
+	 * The normal target frequency for ULP B0 is 500Mhz,
+	 * but ROM set it to 413Mhz, need to change SPLL PFD0 FRAC
+	 */
+	if (soc_rev() >= CHIP_REV_2_0) {
+		/* Switch RCCR SCG to SOSC, firstly check the SOSC is valid */
+		if ((readl(&scg1_regs->sosccsr) & SCG_SOSC_CSR_SOSCVLD_MASK)) {
+			val = readl(&scg1_regs->rccr);
+			val &= (~SCG_CCR_SCS_MASK);
+			val |= ((SCG_SCS_SYS_OSC) << SCG_CCR_SCS_SHIFT);
+			writel(val, &scg1_regs->rccr);
+
+			/* Switch the PLLS to SPLL clk */
+			val = readl(&scg1_regs->spllcfg);
+			val &= ~SCG_PLL_CFG_PLLSEL_MASK;
+			writel(val, &scg1_regs->spllcfg);
+
+			/*
+			 * Re-configure PFD0 to 19,
+			 * A7 SPLL(528MHz) * 18 / 19 = 500MHz
+			 */
+			scg_enable_pll_pfd(SCG_SPLL_PFD0_CLK, 19);
+
+			/* Switch the PLLS to SPLL PFD0 */
+			val = readl(&scg1_regs->spllcfg);
+			val |= SCG_PLL_CFG_PLLSEL_MASK;
+			writel(val, &scg1_regs->spllcfg);
+
+			/* Set RCCR SCG to SPLL clk out */
+			val = readl(&scg1_regs->rccr);
+			val &= (~SCG_CCR_SCS_MASK);
+			val |= ((SCG_SCS_SYS_PLL) << SCG_CCR_SCS_SHIFT);
+			writel(val, &scg1_regs->rccr);
+		}
+	}
+}